#include "LidarDriverHS.h"

#pragma region Init
LidarDriverHS::LidarDriverHS(LidarConfigurationOptions *options, int id)
    : LidarDriverX(options, id),
      cloud0(new pcl::PointCloud<pcl::PointXYZI>),
      cloud1(new pcl::PointCloud<pcl::PointXYZI>),
      cloud2(new pcl::PointCloud<pcl::PointXYZI>)
{
}

void LidarDriverHS::init()
{
    _data_port = std::get<uint16_t>(_options->GetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort));
    _correction_file_path = std::get<std::string>(_options->GetValue(LidarConfigurationOptions::Data_Decode_CorrectionFile));
    _local_ip = std::get<std::string>(_options->GetValue(LidarConfigurationOptions::Data_SrcNet_LocalIP));
    _remote_ip = std::get<std::string>(_options->GetValue(LidarConfigurationOptions::Data_SrcNet_RemoteIP));

    LidarInputSourceType sourceType = std::get<LidarInputSourceType>(_options->GetValue(LidarConfigurationOptions::Data_Source_SrcType));
    switch (sourceType)
    {
    case LidarInputSourceType::ONLINE_LIDAR:
        _is_raw_data = false;
        break;
    default:
        break;
    }

    if (_decodeType == LidarDecodeType::RAW)
        _is_raw_data = true;
}

void LidarDriverHS::dispose()
{
    std::vector<std::tuple<double, double>>().swap(lstFramesBound);
    std::vector<double>().swap(lstAzimuth_Offset);
    std::vector<double>().swap(lstElevation);

    _udpReceiver->dispose();
    delete _udpReceiver;
    _udpReceiver = nullptr;

    cloud0->clear();
    cloud1->clear();
    cloud2->clear();

    lastFrameIndex = 0;
    currentFrameIndex = 0;
    _frame_seq = 0;
}
#pragma endregion

#pragma region Thread
void LidarDriverHS::processPointCloud()
{
    if (!ReadAngleCorrectionFile(_correction_file_path, lstFramesBound, lstAzimuth_Offset, lstElevation))
    {
        _to_exit_process = true;
        return;
    }

    if (_udpReceiver != nullptr)
    {
        _udpReceiver->dispose();
        delete _udpReceiver;
        _udpReceiver = nullptr;
    }

    _udpReceiver = new UdpReceiver(_local_ip, _remote_ip, _data_port);
    _udpReceiver->setCallback(std::bind(&LidarDriverHS::OnDatagramReceived, this, std::placeholders::_1));
    _udpReceiver->start();

    while (!_to_exit_process)
    {
        auto msg = _stuffed_cloud_queue.popWait();
        if (msg.get() == NULL)
            continue;

        pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl_pointcloud->points.swap(msg->cloud_ptr->points);

        PointCloudData<PointXYZI> data;
        decodePointCloud(data, pcl_pointcloud);
        data.sequence = msg->sequence;
        data.frame_id = msg->frame_id;
        data.lidar_id = _lidar_id;

        _free_cloud_queue.push(msg);

        //std::cout << "Data Size: " << data.cloud_ptr->size() << std::endl;

        for (PointCloudDataCallback handler : _pointCloudDataCallback)
        {
            handler(data);
        }
    }
}
#pragma endregion

#pragma region Decode
template <size_t N>
uint32_t LidarDriverHS::ToUInt32(const std::array<std::byte, N> bytes, size_t startIndex)
{
    if (startIndex + 4 > N)
    {
        throw std::out_of_range("Not enough bytes in the array.");
    }
    uint32_t result = 0;
    for (size_t i = 0; i < 4; ++i)
    {
        result += static_cast<uint32_t>(bytes[startIndex + i]) << (i * 8);
    }
    return result;
}

template <size_t N>
int32_t LidarDriverHS::ToInt32(const std::array<std::byte, N> bytes, size_t startIndex)
{
    if (startIndex + 4 > N)
    {
        throw std::out_of_range("Not enough bytes in the array.");
    }
    int32_t result = 0;
    for (size_t i = 0; i < 4; ++i)
    {
        result += static_cast<std::int32_t>(bytes[startIndex + i]) << (i * 8);
    }
    return result;
}

template <size_t N>
int16_t LidarDriverHS::ToInt16(const std::array<std::byte, N> bytes, size_t startIndex)
{
    if (startIndex + 2 > N)
    {
        throw std::out_of_range("Not enough bytes in the array.");
    }
    int16_t result = 0;
    for (size_t i = 0; i < 2; ++i)
    {
        result += (static_cast<std::int32_t>(bytes[startIndex + i]) << (i * 8));
    }

    return result;
}

bool LidarDriverHS::ReadAngleCorrectionFile(std::string path, std::vector<std::tuple<double, double>> &lstFramesBound, std::vector<double> &lstAzimuth_Offset, std::vector<double> &lstElevation)
{
    std::ifstream file(path, std::ios::binary);

    if (!file.is_open())
    {
        throw std::runtime_error("Failed to open file.");
        return false;
    }

    file.seekg(0, std::ios::end);
    std::streamsize fileSize = file.tellg();
    file.seekg(0, std::ios::beg);

    std::vector<std::byte> bytes(fileSize);
    file.read(reinterpret_cast<char *>(bytes.data()), fileSize);

    if (!file)
    {
        throw std::runtime_error("Failed to read file.");
        return false;
    }

    int ProtocolVersionMajor = static_cast<std::uint8_t>(bytes[2]);
    int ProtocolVersionMinor = static_cast<std::uint8_t>(bytes[3]);
    if (!(ProtocolVersionMajor == 1 && ProtocolVersionMinor == 5))
    {
        return false;
    }

    int Resolution = static_cast<std::uint8_t>(bytes[16 - 1]);

    std::array<std::byte, 4 * 3> bytesStart_Frame;
    std::array<std::byte, 4 * 3> bytesEnd_Frame;
    std::array<std::byte, 4 * 128> bytesAzimuth_Offset;
    std::array<std::byte, 4 * 128> bytesElevation;

    std::memcpy(&bytesStart_Frame, bytes.data() + 16, 4 * 3);
    std::memcpy(&bytesEnd_Frame, bytes.data() + 16 + 12, 4 * 3);
    std::memcpy(&bytesAzimuth_Offset, bytes.data() + 16 + 12 * 2, 4 * 128);
    std::memcpy(&bytesElevation, bytes.data() + 16 + 12 * 2 + 4 * 128, 4 * 128);

    auto frame1Start = ToUInt32<bytesStart_Frame.size()>(bytesStart_Frame, 0) * Resolution / (double)25600; // 22
    auto frame2Start = ToUInt32<bytesStart_Frame.size()>(bytesStart_Frame, 4) * Resolution / (double)25600; // 142
    auto frame3Start = ToUInt32<bytesStart_Frame.size()>(bytesStart_Frame, 8) * Resolution / (double)25600;

    auto frame1End = ToUInt32<bytesEnd_Frame.size()>(bytesEnd_Frame, 0) * Resolution / (double)25600; // 142
    auto frame2End = ToUInt32<bytesEnd_Frame.size()>(bytesEnd_Frame, 4) * Resolution / (double)25600; // 262
    auto frame3End = ToUInt32<bytesEnd_Frame.size()>(bytesEnd_Frame, 8) * Resolution / (double)25600; // 22

    lstFramesBound = {
        std::make_tuple(frame1Start, frame1End),
        std::make_tuple(frame2Start, frame2End),
        std::make_tuple(frame3Start, frame3End)};

    for (int i = 1; i <= 128; i++)
    {
        auto offset = ToInt32<bytesAzimuth_Offset.size()>(bytesAzimuth_Offset, (i - 1) * 4) * Resolution / (double)25600;
        lstAzimuth_Offset.push_back(offset);
    }

    for (int i = 1; i <= 128; i++)
    {
        auto offset = ToInt32<bytesAzimuth_Offset.size()>(bytesElevation, (i - 1) * 4) * Resolution / (double)25600;
        lstElevation.push_back(offset);
    }

    return true;
}

void LidarDriverHS::OnDatagramReceived(const std::vector<uint8_t> &bytes)
{
    std::cout << bytes.size() << std::endl;
    if (bytes.size() < 1118)
        return;

    std::array<std::byte, 6> bytesPackageHeader;
    std::array<std::byte, 6> bytesDataHeader;
    std::array<std::byte, 1034> bytesData;
    std::array<std::byte, 40> bytesDataFooter;
    std::array<std::byte, 32> bytesSafe;

    std::memcpy(&bytesPackageHeader, bytes.data(), 6);
    std::memcpy(&bytesDataHeader, bytes.data() + 6, 6);
    std::memcpy(&bytesData, bytes.data() + 6 + 6, 1034);
    std::memcpy(&bytesDataFooter, bytes.data() + 6 + 6 + 1034, 40);
    std::memcpy(&bytesSafe, bytes.data() + 6 + 6 + 1034 + 40, 32);

    std::byte ProtocolVersionMajor = bytesPackageHeader[2];
    std::byte ProtocolVersionMinor = bytesPackageHeader[3];

    if (!(static_cast<int>(ProtocolVersionMajor) == 4 && static_cast<int>(ProtocolVersionMinor) == 3))
        return;

    std::byte ChannelNum = bytesDataHeader[0]; // 激光通道数128
    std::byte BlockNum = bytesDataHeader[1];
    std::byte DisUnit = bytesDataHeader[3];
    std::byte ReturnNum = bytesDataHeader[4];

    uint16_t Azimuth1 = ToInt16<bytesData.size()>(bytesData, 0);
    std::byte FineAzimuth1 = bytesData[2];

    std::array<std::byte, 512> DataBlock1;
    std::memcpy(&DataBlock1, bytesData.data() + 3, 512);
    double az1 = Azimuth1 * 0.01 + static_cast<int>(FineAzimuth1) * 0.01 / 256; // 码盘角度

    SetPoints(DataBlock1, static_cast<int>(DisUnit) * 0.001, az1, lstFramesBound, lstAzimuth_Offset, lstElevation);

    uint16_t Azimuth2 = ToInt16<bytesData.size()>(bytesData, 515);
    std::byte FineAzimuth2 = bytesData[515 + 2];

    std::array<std::byte, 512> DataBlock2;
    std::memcpy(&DataBlock2, bytesData.data() + 515 + 3, 512);
    double az2 = Azimuth2 * 0.01 + static_cast<int>(FineAzimuth2) * 0.01 / 256; // 码盘角度

    SetPoints(DataBlock2, static_cast<int>(DisUnit) * 0.001, az2, lstFramesBound, lstAzimuth_Offset, lstElevation);
}

bool LidarDriverHS::SetPoints(std::array<std::byte, 512> bytes, double DisUnit, double azimuth, std::vector<std::tuple<double, double>> lstFramesBound, std::vector<double> lstAzimuth_Offset, std::vector<double> lstElevation)
{
    int frameIndex = -1;
    for (int i = 0; i < 3; i++)
    {
        auto bound = lstFramesBound[i];
        if (azimuth >= std::get<0>(bound) && azimuth <= std::get<1>(bound))
        {
            frameIndex = i;
        }
        else if (std::get<0>(bound) > std::get<1>(bound))
        {
            if (azimuth >= std::get<0>(bound) || azimuth <= std::get<1>(bound))
            {
                frameIndex = i;
            }
        }
    }

    auto a = azimuth - std::get<0>(lstFramesBound[frameIndex]);
    if (a < 0)
        a += 360;
    auto baseHAngle = a * 2; // 码盘转1°   水平方位角变化2°
    // var hIndex = (int)((baseHAngle - 30) / 0.1);
    std::round((baseHAngle - 30) * 10);
    auto hIndex = (int)std::round((baseHAngle - 30) * 10);
    if (!(hIndex >= 0 && hIndex <= 1200 - 1))
    {
        return false;
    }

    auto x = bytes.size();
    for (int j = 0; j < bytes.size() / 4; j++)
    {
        int startIndex = j * 4;
        auto point = LidarPoint();

        point.Distance = ToInt16<bytes.size()>(bytes, startIndex);
        point.Reflectivity = bytes[startIndex + 2];

        auto hAngle = baseHAngle - lstAzimuth_Offset[j];
        auto vAngle = lstElevation[j];

        int index = 1200 * j + hIndex;

        point.z = static_cast<int16_t>((std::sin(vAngle * M_PI / 180.0) * point.Distance * DisUnit * 100));
        auto xy = std::cos(vAngle * M_PI / 180.0) * point.Distance * DisUnit;
        point.x = static_cast<int16_t>((xy * std::sin(hAngle * M_PI / 180.0) * 100));
        point.y = static_cast<int16_t>((xy * std::cos(hAngle * M_PI / 180.0) * 100));

        if (!_is_raw_data && point.x == 0 && point.y == 0 && point.z == 0)
            continue;

        // std::cout << "1:" << point.x << " " << point.y << " " << point.z << std::endl;
        // std::cout << "2:" << (double)point.x / 100.0 << " " << (double)point.y / 100.0 << " " << (double)point.z / 100.0 << std::endl;

        if (frameIndex == 0)
            cloud0->push_back(pcl::PointXYZI((double)point.x / 100.0, (double)point.y / 100.0, (double)point.z / 100.0));
        else if (frameIndex == 1)
            cloud1->push_back(pcl::PointXYZI((double)point.x / 100.0, (double)point.y / 100.0, (double)point.z / 100.0));
        else if (frameIndex == 2)
            cloud2->push_back(pcl::PointXYZI((double)point.x / 100.0, (double)point.y / 100.0, (double)point.z / 100.0));
    }

    currentFrameIndex = frameIndex;

    // 一帧完成
    if (lastFrameIndex != currentFrameIndex)
    {
        _frame_seq++;

        auto data = getPointCloud();
        data->sequence = _frame_seq;
        auto now = std::chrono::system_clock::now();
        auto timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
        data->timestamp = (double)timestamp / 1000.0;

        if (frameIndex == 0)
        {
            pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
            cloud->points.swap(cloud2->points);
            data->cloud_ptr = cloud;
            data->frame_id = "frame_" + std::to_string(frameIndex);
        }
        else if (frameIndex == 1)
        {
            pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
            cloud->points.swap(cloud0->points);
            data->cloud_ptr = cloud;
            data->frame_id = "frame_" + std::to_string(frameIndex);
        }
        else if (frameIndex == 2)
        {
            pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
            cloud->points.swap(cloud1->points);
            data->cloud_ptr = cloud;
            data->frame_id = "frame_" + std::to_string(frameIndex);
        }

        setPointCloud(data);
        lastFrameIndex = currentFrameIndex;
    }

    return true;
}

std::shared_ptr<PointCloudData<pcl::PointXYZI>> LidarDriverHS::getPointCloud()
{
    std::shared_ptr<PointCloudData<pcl::PointXYZI>> data = _free_cloud_queue.pop();
    if (data.get() != NULL)
        return data;

    return std::make_shared<PointCloudData<pcl::PointXYZI>>();
}

void LidarDriverHS::setPointCloud(std::shared_ptr<PointCloudData<pcl::PointXYZI>> data)
{
    auto len = _stuffed_cloud_queue.push(data);
    if (len > max_buffer_num)
        _stuffed_cloud_queue.pop();
}
#pragma endregion